#ifndef H_ACC
#define H_ACC

#include "Arduino.h"

class Acc
{
private:
  float x0,y0,z0;
  int xi,yi,zi;
  byte state;

public:
  int errorCount;
  byte errorCode;
  float x,y,z;


  byte setup()
  {
    byte ret;

#if defined(BMA180)
    ret=fw.write(ACC_ADDR,0x0D,1<<4);
    if (ret!=0) return 1;

    byte control;
    fw.readBuf(ACC_ADDR,0x20,&control,1);
    fw.write(ACC_ADDR,0x20,control & 0x0f);

#elif defined(MPU6050)
    ret=fw.write(ACC_ADDR, 0x1C, 0x10);
    if (ret!=0) return 1;
#endif
    z0=4096;
    return 0;
  }

  byte update()
  {
    byte ret;
    byte buff[4*sizeof(int)];

#if defined(BMA180)

    ret=fw.readBuf(ACC_ADDR, 0x02, buff, 4*sizeof(int));
    if (ret==0)
    {
      ACC_ORIENTATION(
      ((buff[3] << 8) | buff[2])>>2, 
      -((buff[1] << 8) | buff[0])>>2, 
      ((buff[5] << 8) | buff[4])>>2);
    }
#elif defined(MPU6050)

    ret=fw.readBuf(ACC_ADDR, 0x3b, buff, 3*sizeof(int));
    if (ret==0)
    {
      ACC_ORIENTATION(
      ((buff[0] << 8) | buff[1]), 
      ((buff[2] << 8) | buff[3]), 
      ((buff[4] << 8) | buff[5]));
    }
#else
    if (true)
    {
    }
#endif
    else
    {
      errorCount++;
      errorCode=ret;
      xi=x0;
      yi=y0;
      zi=z0;
    }

    if (state==1) // 1=search zero
    {
      x0=xi;
      y0=yi;
      z0=zi;
      state=0;
      saveAll();
      led.setOutput(BLINKNORMAL);
    }

    x=(xi-x0)/z0;
    y=(yi-y0)/z0;
    z=zi/z0;

    return 0;
  }



  void setState(byte s)
  {
    state=s;
  }

  void save(void)
  {
    para.writeFloat(x0);
    para.writeFloat(y0);
    para.writeFloat(z0);
  }

  void load(void)
  {
    x0=para.readFloat();
    y0=para.readFloat();
    z0=para.readFloat();
  }
} 
acc;

#endif






















